#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "queue.h"

#define TAG "queue: "
#define p_queue_debug_log(TAG, ...) //do { printf("%s", TAG); printf(__VA_ARGS__); printf("\n"); } while(0)
#define p_queue_info_log(TAG, ...)  //do { printf("%s", TAG); printf(__VA_ARGS__); printf("\n"); } while(0)

/**
 * @brief A safe memory allocation function
 * 
 * This function attempts to allocate memory of the specified size. If the allocation fails,
 * an error message is printed.
 * 
 * @param size The number of bytes to allocate
 * @return void* Pointer to the allocated memory, or NULL if allocation failed
 */
static void *queue_malloc(size_t size) {
    if (size == 0) return NULL;
    void *mem = malloc(size);
    if (!mem) {
        p_queue_debug_log(TAG, "Memory allocation failed for size: %zu", size);
    }
    return mem;
}

/**
 * @brief Frees allocated memory
 * 
 * This function frees the memory pointed to by the given pointer.
 * 
 * @param mem Pointer to the memory to be freed
 */
static void queue_free(void *mem) {
    if (mem) {
        free(mem);
    }
}

/**
 * @brief Initializes the mutex for queue access synchronization
 * 
 * This function initializes the queue mutex, setting the lock state to false (unlocked).
 * 
 * @param mutex Pointer to the mutex structure to initialize
 */
static void queue_mutex_init(queue_mutex_t *mutex) {
    if (mutex) {
        mutex->locked = false;  // Mutex is initially unlocked
    }
}

/**
 * @brief Locks the queue mutex to prevent concurrent access
 * 
 * This function locks the mutex, ensuring that no other tasks can access the queue 
 * while it is locked. This is a simplified version of mutex locking.
 * 
 * @param mutex Pointer to the mutex structure to lock
 */
static void queue_mutex_lock(queue_mutex_t *mutex) {
    if (mutex) {
        // while (mutex->locked) {
        // }

        // Acquire the lock
        mutex->locked = true;
    }
}

/**
 * @brief Unlocks the queue mutex to allow concurrent access
 * 
 * This function unlocks the mutex, allowing other tasks to access the queue.
 * 
 * @param mutex Pointer to the mutex structure to unlock
 */
static void queue_mutex_unlock(queue_mutex_t *mutex) {
    if (mutex) {
        // Release the lock
        mutex->locked = false;
    }
}

/**
 * @brief Creates a new queue
 * 
 * This function dynamically allocates memory for a new queue and initializes it with
 * the specified data size and item type size.
 * 
 * @param data_size The maximum number of items the queue can hold
 * @param data_type The size of each item in the queue (in bytes)
 * @return queue_t* Pointer to the created queue, or NULL if memory allocation failed
 */
queue_t *queue_create(uint32_t data_size, uint32_t data_type) {
    if (data_size == 0 || data_type == 0) {
        p_queue_debug_log(TAG, "Error: Invalid data_size (%u) or data_type (%u)", data_size, data_type);
        return NULL;
    }

    p_queue_debug_log(TAG, "Creating queue with max_size: %u, max_type_size: %u", data_size, data_type);

    queue_t *q = (queue_t *)queue_malloc(sizeof(queue_t));
    if (!q) {
        p_queue_debug_log(TAG, "Error: Memory allocation failed for queue structure");
        return NULL;
    }

    q->cur_size = 0;
    q->max_type_size = data_type;
    q->max_size = data_size;

    p_queue_debug_log(TAG, "Allocating memory for queue buffer with size: %u bytes", data_size * data_type);
    
    q->pc_head = (char *)queue_malloc(data_size * data_type);
    if (!q->pc_head) {
        p_queue_debug_log(TAG, "Error: Memory allocation failed for queue buffer");
        queue_free(q);
        return NULL;
    }

    q->pc_tail = q->pc_head + (data_size * data_type);
    q->pc_read = q->pc_head;
    q->pc_write = q->pc_head;

    queue_mutex_init(&q->queue_mutex);
    p_queue_debug_log(TAG, "Queue created successfully. Head: %p, Tail: %p, Read: %p, Write: %p", q->pc_head, q->pc_tail, q->pc_read, q->pc_write);

    return q;
}

/**
 * @brief Destroys the queue and frees allocated memory
 * 
 * This function frees the memory allocated for the queue structure and its data storage.
 * 
 * @param q Pointer to the queue to destroy
 */
void queue_destory(queue_t **q) {
    if (!q || !*q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL or already freed");
        return;
    }

    queue_t *f_q = *q;

    p_queue_debug_log(TAG, "Destroying queue. Queue pointer: %p", f_q);

    if (f_q->pc_head) {
        p_queue_debug_log(TAG, "Freeing queue buffer at %p", f_q->pc_head);
        queue_free(f_q->pc_head);
    }

    queue_free(f_q);
    *q = NULL;

    p_queue_debug_log(TAG, "Queue destroyed successfully");
}

/**
 * @brief Checks if the queue is full
 * 
 * This function checks if the current number of items in the queue has reached the 
 * maximum allowed size.
 * 
 * @param q Pointer to the queue
 * @return true if the queue is full, false otherwise
 */
bool queue_is_full(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return false;
    }

    bool is_full = q->cur_size == q->max_size;
    p_queue_debug_log(TAG, "Queue full check: cur_size = %u, max_size = %u, is_full = %s", q->cur_size, q->max_size, is_full ? "true" : "false");
    
    return is_full;
}

/**
 * @brief Checks if the queue is empty
 * 
 * This function checks if the queue contains any items.
 * 
 * @param q Pointer to the queue
 * @return true if the queue is empty, false otherwise
 */
bool queue_is_empty(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return false;
    }

    bool is_empty = q->cur_size == 0;
    p_queue_debug_log(TAG, "Queue empty check: cur_size = %u, is_empty = %s", q->cur_size, is_empty ? "true" : "false");

    return is_empty;
}

/**
 * @brief Sends a message to the queue, overwriting the oldest message if full
 * 
 * This function inserts a new message into the queue. If the queue is full, it will 
 * overwrite the oldest message (cover behavior).
 * 
 * @param q Pointer to the queue
 * @param message Pointer to the message to send
 * @return queue_status_t QUEUE_SUCCESS if the message was successfully added, 
 *                        QUEUE_NULL_POINTER if the queue pointer is NULL, 
 *                        QUEUE_FULL if the queue is full
 */
queue_status_t queue_send_cover(queue_t *q, const void *message) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return QUEUE_NULL_POINTER;
    }

    p_queue_debug_log(TAG, "Sending message to queue. Current size: %u, Max size: %u", q->cur_size, q->max_size);

    queue_mutex_lock(&q->queue_mutex);

    p_queue_debug_log(TAG, "Writing message to queue at address: %p", q->pc_write);
    memcpy(q->pc_write, message, q->max_type_size);
    q->pc_write += q->max_type_size;

    // Wrap around the write pointer if it reaches the end of the buffer
    if (q->pc_write >= q->pc_tail) {
        p_queue_debug_log(TAG, "Write pointer reached the end, wrapping around");
        q->pc_write = q->pc_head;
    }

    if (!queue_is_full(q)) {
        q->cur_size++;
        p_queue_debug_log(TAG, "Message added. New size: %u", q->cur_size);
    } else {
        p_queue_debug_log(TAG, "Queue is full, unable to add message");
    }

    queue_mutex_unlock(&q->queue_mutex);

    return QUEUE_SUCCESS;
}

/**
 * @brief Clears the queue (removes all messages)
 * 
 * This function clears the queue by resetting the read and write pointers and setting the 
 * current size to zero. This operation does not free the memory allocated for the queue.
 * 
 * @param q Pointer to the queue
 */
void queue_clear(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return;
    }

    queue_mutex_lock(&q->queue_mutex);

    p_queue_debug_log(TAG, "Clearing the queue");
    q->cur_size = 0;
    q->pc_read = q->pc_head;
    q->pc_write = q->pc_head;

    queue_mutex_unlock(&q->queue_mutex);
}
/**
 * @brief Sends a message to the queue
 * 
 * This function inserts a new message into the queue, but only if the queue is not full.
 * 
 * @param q Pointer to the queue
 * @param message Pointer to the message to send
 * @return queue_status_t QUEUE_SUCCESS if the message was successfully added, 
 *                        QUEUE_NULL_POINTER if the queue pointer is NULL, 
 *                        QUEUE_FULL if the queue is full
 */
queue_status_t queue_send(queue_t *q, const void *message) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return QUEUE_NULL_POINTER;
    }

    if (queue_is_full(q)) {
        p_queue_debug_log(TAG, "Error: Queue is full. Cannot add message.");
        return QUEUE_FULL;
    }

    queue_mutex_lock(&q->queue_mutex);

    p_queue_debug_log(TAG, "Sending message to queue at %p", q->pc_write);
    memcpy(q->pc_write, message, q->max_type_size);
    q->pc_write += q->max_type_size;

    if (q->pc_write >= q->pc_tail) {
        q->pc_write = q->pc_head;
    }

    q->cur_size++;
    p_queue_debug_log(TAG, "Message sent. New size: %u", q->cur_size);

    queue_mutex_unlock(&q->queue_mutex);

    return QUEUE_SUCCESS;
}



/**
 * @brief Receives a message from the queue
 * 
 * This function retrieves the next message from the queue and removes it from the queue.
 * If the queue is empty, it returns an error indicating that there are no messages to receive.
 * The message is returned by updating the pointer passed as an argument.
 * 
 * @param q Pointer to the queue
 * @param message Pointer to a variable where the received message will be stored
 * @return queue_status_t Returns QUEUE_SUCCESS if the message was successfully received, 
 *                        QUEUE_NULL_POINTER if the queue pointer is NULL, 
 *                        QUEUE_EMPTY if the queue is empty
 */
queue_status_t queue_receive(queue_t *q, void **message) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return QUEUE_NULL_POINTER;
    }

    if (queue_is_empty(q)) {
        p_queue_debug_log(TAG, "Error: Queue is empty. No message to receive.");
        return QUEUE_EMPTY;
    }

    p_queue_debug_log(TAG, "Receiving message from queue. Current size: %u, Max size: %u", q->cur_size, q->max_size);

    queue_mutex_lock(&q->queue_mutex);

    // Assign the read pointer to the message
    *message = q->pc_read;
    p_queue_debug_log(TAG, "Message read from queue at address: %p", q->pc_read);

    q->cur_size--;
    q->pc_read += q->max_type_size;

    // Wrap around the read pointer if it reaches the end of the buffer
    if (q->pc_read >= q->pc_tail) {
        p_queue_debug_log(TAG, "Read pointer reached the end, wrapping around");
        q->pc_read = q->pc_head;
    }

    p_queue_debug_log(TAG, "Message successfully received. New size: %u", q->cur_size);

    queue_mutex_unlock(&q->queue_mutex);

    return QUEUE_SUCCESS;
}
/**
 * @brief Receives a message from the queue and copies it to the provided buffer.
 *
 * This function attempts to receive a message from the specified queue. If successful, 
 * the message is copied into the provided buffer, with the size determined by the 
 * `max_type_size` field of the queue structure. The function does not allocate new memory 
 * for the message, but instead copies it into an existing buffer provided by the caller.
 *
 * @param q [in] A pointer to the queue from which the message will be retrieved.
 * @param message [out] A pointer to the buffer where the received message will be copied.
 * 
 * @return queue_status_t The status of the receive operation:
 *         - `QUEUE_SUCCESS` on success, meaning a message was received and copied to the buffer.
 *         - Other status codes may indicate errors such as queue being empty or other failure conditions.
 */

queue_status_t queue_receive_value(queue_t *q, void *message){
    void *msg = NULL;
    queue_status_t status;

    status = queue_receive(q, &msg);
    if (status == QUEUE_SUCCESS){
        memcpy(message, msg, q->max_type_size);
    }

    return status;
}
/**
 * @brief Peeks at the front of the queue without removing the item
 * 
 * This function allows inspecting the next item in the queue without removing it.
 * It simply provides a view of the message at the front of the queue while leaving it intact.
 * If the queue is empty, it returns an error indicating that there is no item to peek.
 * 
 * @param q Pointer to the queue
 * @param message Pointer to a variable where the peeked message will be stored
 * @return queue_status_t Returns QUEUE_SUCCESS if the message was successfully peeked, 
 *                        QUEUE_NULL_POINTER if the queue pointer is NULL, 
 *                        QUEUE_EMPTY if the queue is empty
 */
queue_status_t queue_peek(queue_t *q, void **message) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL. Cannot peek.");
        return QUEUE_NULL_POINTER;
    }

    if (queue_is_empty(q)) {
        p_queue_debug_log(TAG, "Error: Queue is empty. Cannot peek.");
        return QUEUE_EMPTY;
    }

    queue_mutex_lock(&q->queue_mutex);

    *message = q->pc_read;
    p_queue_debug_log(TAG, "Peeked at the queue. Current read position: %p, Message: %d", *message, *(int *)(*message));

    queue_mutex_unlock(&q->queue_mutex);

    return QUEUE_SUCCESS;
}
/**
 * @brief Peeks at the next message in the queue and copies it to the provided buffer.
 *
 * This function attempts to peek at the next message in the specified queue without removing 
 * it from the queue. If successful, the message is copied into the provided buffer, with 
 * the size determined by the `max_type_size` field of the queue structure. The queue state is 
 * unchanged after this operation, meaning the message remains in the queue.
 *
 * @param q [in] A pointer to the queue from which the message will be peeked.
 * @param message [out] A pointer to the buffer where the peeked message will be copied.
 * 
 * @return queue_status_t The status of the peek operation:
 *         - `QUEUE_SUCCESS` on success, meaning a message was successfully peeked and copied.
 *         - Other status codes may indicate errors such as the queue being empty or other failure conditions.
 */
queue_status_t queue_peek_value(queue_t *q, void *message){
    void *msg = NULL;
    queue_status_t status;

    status = queue_peek(q, &msg);
    if (status == QUEUE_SUCCESS){
        memcpy(message, msg, q->max_type_size);
    }

    return status;
}
/**
 * @brief Retrieves the current size of the queue
 * 
 * This function returns the number of items currently stored in the queue.
 * 
 * @param q Pointer to the queue
 * @return uint32_t The current number of items in the queue
 */
uint32_t queue_size(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL. Returning size as 0.");
        return 0;
    }

    p_queue_debug_log(TAG, "Queue size requested. Current size: %u, Max size: %u", q->cur_size, q->max_size);
    
    return q->cur_size;
}

/**
 * @brief Retrieves the maximum capacity of the queue
 * 
 * This function returns the maximum number of items that the queue can hold.
 * 
 * @param q Pointer to the queue
 * @return uint32_t The maximum size of the queue
 */
uint32_t queue_get_max_size(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return 0;
    }

    return q->max_size;
}
/**
 * @brief Retrieves the maximum size of each item in the queue
 * 
 * This function returns the maximum size of each item stored in the queue.
 * 
 * @param q Pointer to the queue
 * @return uint32_t The size of each item in the queue
 */
uint32_t queue_get_item_size(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return 0;
    }

    return q->max_type_size;
}

/**
 * @brief Checks if the queue is locked
 * 
 * This function checks if the queue's mutex is locked, which is useful for debugging concurrent
 * access to the queue.
 * 
 * @param q Pointer to the queue
 * @return bool true if the queue is locked, false otherwise
 */
bool queue_is_locked(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return false;
    }

    return q->queue_mutex.locked;
}

/**
 * @brief Debugging function to print the state of the queue
 * 
 * This function prints the current state of the queue, including the current size, read and 
 * write pointers, and buffer contents. It is useful for debugging and development.
 * 
 * @param q Pointer to the queue
 */
void queue_print_debug_info(queue_t *q) {
    if (!q) {
        p_queue_debug_log(TAG, "Error: Queue pointer is NULL");
        return;
    }

    p_queue_debug_log(TAG, "Queue state: cur_size = %u, max_size = %u, max_type_size = %u", 
                      q->cur_size, q->max_size, q->max_type_size);
    p_queue_debug_log(TAG, "Read pointer: %p, Write pointer: %p", q->pc_read, q->pc_write);
    p_queue_debug_log(TAG, "Queue buffer: Head: %p, Tail: %p", q->pc_head, q->pc_tail);
}

/**
 * @brief Prints the current contents of the queue
 * 
 * This function prints the messages currently in the queue, starting from the front.
 * 
 * @param q Pointer to the queue
 */
void queue_print(queue_t *q) {
    if (!q) {
        return;
    }

    char *message_p = q->pc_read;
    printf("message queue print: ");
    for (int i = 0; i < q->cur_size; i++) {
        printf("%d ", *(int *)(message_p + (i * q->max_type_size)));
    }
    printf("\n");
}
